package uk.ac.cam.cl.oscar.edgefinder;

import java.lang.Math;
import java.util.*;

public class LineFinder {

	/* ## CONSTANTS ## */

	/* Rho ranges from -1.0 to 1.0 */
	private static final int rhoResolution = 800;
	private static final float rhoRange = 2.0f;
	private static final float rhoBias = -1.0f;
	private static final float rhoMul = rhoRange/(float)rhoResolution;
	
	/* rhoValue = rhoCellNumber * rhoMul + rhoBias */
	
	/* Theta ranges from 0 to pi */
	private static final int thetaResolution = 600;
	private static final float thetaRange = 2.0f*(float)Math.PI;
	private static final float thetaMul = thetaRange/(float)thetaResolution;
	
	/* Number of cells in the voting grid */
	private static final int cellCount = rhoResolution * thetaResolution;
	
	/* ## CLASS MEMBERS ## */
	private float[] votePlane;
	private float[] rhoVector;
	private List points;
	private float var;
	private float asq;
	private int rhoVectorSize;
	private int vectorDecayLength;
	
	private float sq (float x) { return x*x; }
	
	/* @variance Variance. Try 0.003
	   @asquared A Squared - a measure of how noise tolerant the filter is. Try 0.006
	   */
	public LineFinder (float variance, float asquared) {
		var = variance;
		asq = asquared;
		
		votePlane = new float[cellCount];
		
		
		/* Set up what looks sort of like a 1d gaussian slice. It is the voting template
		   for every point, as they vote for lines that pass near them, although with a lower
		   value bvote */
		   
		/* The number of values above and below the maximum vector point */
		vectorDecayLength = (int)Math.ceil(Math.sqrt(asq * var) / rhoMul);
		vectorDecayLength = Math.min( vectorDecayLength, rhoResolution );
	
		/* This will hold a column of vote values to add to a theta */
		rhoVectorSize = 2*vectorDecayLength + 1;
		rhoVector = new float[rhoVectorSize];
		
		/* Cell values are from asq to 0 in steps of 1/(res*var)^2 */
		rhoVector[vectorDecayLength] = asq;
		for (int i = 0; i < vectorDecayLength; i++) {
			rhoVector[i] = rhoVector[rhoVectorSize-1-i] = Math.max(asq - sq((float)(vectorDecayLength-i)*rhoMul)/var, 0);
		}
		
		points = new ArrayList (600);
	}
	
	public void addPoint (NormalisedPoint p) {
		points.add (p);
	}

	public void clearPoints () { points.clear(); }
	
	
	/** Read the paper 'Whats in a set of points' or just wikipedia on 'Hough Transform'
	    This is just a Hough Transform on the line properties, with some little neat
	    extras and a decent efficient implementation
	    */
	public RhoLine findLine () 
	{
		if (points.size() == 0) return new RhoLine (0,0);
		
		for (int i = 0; i < cellCount; i++) votePlane[i] = 0.0f;
	
		NormalisedPoint p = (NormalisedPoint) points.get(0);
		for (Iterator i = points.iterator(); i.hasNext(); p = (NormalisedPoint)i.next()) 
		{
			for (int theta = 0; theta < thetaResolution; theta++) 
			{
				double rhoi = p.normalX() * Math.cos((double)theta*thetaMul) + p.normalY() * Math.sin((double)theta*thetaMul);
				
				// Add this to the rho index to get the rhoVectorIndex
				int rhoToIndex = -(int)Math.floor((rhoi - rhoBias) / rhoMul) + vectorDecayLength;
				
				for (int rho = 0; rho < rhoResolution; rho++) 
				{
					int index = rhoToIndex + rho;
					
					if (index >= 0 && index < rhoVectorSize) {
						votePlane[rho + theta * rhoResolution] += rhoVector[index];
					}
				}
			}
		}
		
		float maxC = 0;
		float bestRho = 0;
		float bestTheta = 0;
		int index = 0;
		
		for (int theta = 0; theta < thetaResolution; theta++) {
			for (int rho = 0; rho < rhoResolution; rho++) {
				index = rho + theta*rhoResolution;
				
				if (votePlane[index] > maxC) {
					maxC = votePlane[index];
					bestRho = (float)rho * rhoMul + rhoBias;
					bestTheta = (float)theta * thetaMul;
				}
			}
		}
		
		return new RhoLine (bestRho, bestTheta);
	}
}